#include <ros/ros.h> //ros头文件
#include <cv_bridge/cv_bridge.h> //图像转化头文件
#include <sensor_msgs/image_encodings.h> //图像编码格式头文件
#include <opencv2/imgproc/imgproc.hpp> //图像处理头文件
#include <opencv2/highgui/highgui.hpp> //图像化显示头文件

using namespace cv; //使用命名空间 在当前代码范围内，我可以直接使用cv命名空间中的所有名称，而不必每次都写cv::前缀
using namespace std;

static int iLowH = 10;
static int iHighH = 40;

static int iLowS = 90;
static int iHighS = 255;

static int iLowV = 1;
static int iHighV = 255;

void Cam_RGB_Callback(const sensor_msgs::Image msg)
{
    //数据转和数据存储
    cv_bridge::CvImagePtr cv_ptr;//图像类型指针
    try//保护程序，出现错误可以返回对应错误，保证程序不崩
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);//图像格式转化
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge excpption: %s", e.what());
        return;
    }

    Mat imgOriginal = cv_ptr->image;//创建一个opcv的格式图像信息

    Mat imgHSV;
    cvtColor(imgOriginal,imgHSV,COLOR_BGR2HSV);//将rgb数据转化成hsv并存在imgHSV中

    //在hsv中作直方图均衡化
    vector<Mat> hsvSplit;
    split(imgHSV, hsvSplit);
    equalizeHist(hsvSplit[2], hsvSplit[2]);
    merge(hsvSplit,imgHSV);

    //使用阈值范围对图像二值化
    Mat imgThresholded;
    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);

    //开操作(去除一些噪点)
    Mat element = getStructuringElement(MORPH_RECT, Size(5,5));
    morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);

    //闭操作(连接一些连通区域)
    morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);

    //便利二值化后的数据
    int nTargetX = 0;//存放预期的xy坐标和数量
    int nTargetY = 0;
    int nPixCount = 0;
    int nImgWidth = imgThresholded.cols;
    int nImgHeight = imgThresholded.rows;
    int nImgChannels = imgThresholded.channels();//通道数量，每个像素占用了几个字节
    for(int y = 0; y < nImgHeight; y++)
    {
        for(int x = 0; x < nImgWidth; x++)
        {
            if(imgThresholded.data[y*nImgWidth + x] == 255)
            {
                nTargetX += x;
                nTargetY += y;
                nPixCount++;
            }
        }
    }
    if(nPixCount > 0)
    {
        nTargetX /= nPixCount;
        nTargetY /= nPixCount;
        printf("颜色质心坐标( %d , %d)  点数 = %d\n",nTargetX,nTargetY,nPixCount);
        //画坐标
        Point line_begin = Point(nTargetX-10,nTargetY);
        Point line_end = Point(nTargetX+10,nTargetY);
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0));
        line_begin.x = nTargetX;line_begin.y = nTargetY+10;
        line_end.x = nTargetX;line_end.y = nTargetY+10;
        line(imgOriginal, line_begin, line_end, Scalar(255,0,0));
    }
    else 
    {
        printf("目标颜色消失。。。、\n");
    }
    


    imshow("RGB", imgOriginal);//将图像显示到rgb的窗口中
    imshow("HSV", imgHSV);//将图像显示到rgb的窗口中
    imshow("Result", imgThresholded);//将图像显示到rgb的窗口中
    waitKey(5);//暂停1毫秒让回调函数等议会
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "cv_image_node");
    
    ros::NodeHandle nh;
    ros::Subscriber rgb_sub = nh.subscribe("/kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback);

    //生成一可以显示和调节参数的窗口
    namedWindow("Threshold", WINDOW_AUTOSIZE);
    createTrackbar("LowH", "Threshold", &iLowH, 179);
    createTrackbar("HighH", "Threshold", &iHighH, 179);

    createTrackbar("LowS", "Threshold", &iLowS, 255);
    createTrackbar("HighS", "Threshold", &iHighS, 255);

    createTrackbar("LowV", "Threshold", &iLowV, 255);
    createTrackbar("HighV", "Threshold", &iHighV, 255);


    namedWindow("RGB"); //创建一个RGB的窗口显示图像
    namedWindow("HSV");
    namedWindow("Result");

    //ros::spin();  //让主函数处于阻塞状态，保持运行   

    //和ros::spin()相同的功能
    ros::Rate loop_rate(30);
    while (ros::ok)
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
                                         
}